home *** CD-ROM | disk | FTP | other *** search
/ Tech Arsenal 1 / Tech Arsenal (Arsenal Computer).ISO / tek-19 / iritsm3s.zip / GENMAT.C < prev    next >
C/C++ Source or Header  |  1992-01-20  |  6KB  |  160 lines
  1. /*****************************************************************************
  2. * General matrix manipulation routines.                         *
  3. *                                         *
  4. * Written by:  Gershon Elber                   Ver 1.0,    June 1988    *
  5. *****************************************************************************/
  6.  
  7. #include <math.h>
  8. #include "irit_sm.h"
  9. #include "genmat.h"
  10.  
  11. /*****************************************************************************
  12. * Routine to generate a 4*4 unit matrix:                                     *
  13. *****************************************************************************/
  14. void GenUnitMat(MatrixType Mat)
  15. {
  16.     int i, j;
  17.  
  18.     for (i = 0; i < 4; i++) for (j = 0; j < 4; j++)
  19.     if (i == j)
  20.         Mat[i][j] = 1.0;
  21.     else
  22.         Mat[i][j] = 0.0;
  23. }
  24.  
  25. /*****************************************************************************
  26. * Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts.       *
  27. *****************************************************************************/
  28. void GenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat)
  29. {
  30.      GenUnitMat(Mat);                                /* Make it unit matrix. */
  31.      Mat[3][0] = Tx;
  32.      Mat[3][1] = Ty;
  33.      Mat[3][2] = Tz;
  34. }
  35.  
  36. /*****************************************************************************
  37. * Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts.   *
  38. *****************************************************************************/
  39. void GenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat)
  40. {
  41.      GenUnitMat(Mat);                                /* Make it unit matrix. */
  42.      Mat[0][0] = Sx;
  43.      Mat[1][1] = Sy;
  44.      Mat[2][2] = Sz;
  45. }
  46.  
  47. /*****************************************************************************
  48. * Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
  49. * Note Teta must be given in radians.                                        *
  50. *****************************************************************************/
  51. void GenMatRotX1(RealType Teta, MatrixType Mat)
  52. {
  53.     RealType CTeta, STeta;
  54.  
  55.     CTeta = cos(Teta);
  56.     STeta = sin(Teta);
  57.     GenMatRotX(CTeta, STeta, Mat);
  58. }
  59.  
  60. /*****************************************************************************
  61. * Routine to generate a 4*4 matrix to Rotate around X with angle of Teta.    *
  62. *****************************************************************************/
  63. void GenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  64. {
  65.      GenUnitMat(Mat);                                /* Make it unit matrix. */
  66.      Mat[1][1] = CosTeta;
  67.      Mat[1][2] = SinTeta;
  68.      Mat[2][1] = -SinTeta;
  69.      Mat[2][2] = CosTeta;
  70. }
  71.  
  72. /*****************************************************************************
  73. * Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
  74. * Note Teta must be given in radians.                                        *
  75. *****************************************************************************/
  76. void GenMatRotY1(RealType Teta, MatrixType Mat)
  77. {
  78.     RealType CTeta, STeta;
  79.  
  80.     CTeta = cos(Teta);
  81.     STeta = sin(Teta);
  82.     GenMatRotY(CTeta, STeta, Mat);
  83. }
  84.  
  85. /*****************************************************************************
  86. * Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta.    *
  87. *****************************************************************************/
  88. void GenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  89. {
  90.      GenUnitMat(Mat);                                /* Make it unit matrix. */
  91.      Mat[0][0] = CosTeta;
  92.      Mat[0][2] = -SinTeta;
  93.      Mat[2][0] = SinTeta;
  94.      Mat[2][2] = CosTeta;
  95. }
  96.  
  97. /*****************************************************************************
  98. * Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
  99. * Note Teta must be given in radians.                                        *
  100. *****************************************************************************/
  101. void GenMatRotZ1(RealType Teta, MatrixType Mat)
  102. {
  103.     RealType CTeta, STeta;
  104.  
  105.     CTeta = cos(Teta);
  106.     STeta = sin(Teta);
  107.     GenMatRotZ(CTeta, STeta, Mat);
  108. }
  109.  
  110. /*****************************************************************************
  111. * Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta.    *
  112. *****************************************************************************/
  113. void GenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat)
  114. {
  115.      GenUnitMat(Mat);                                /* Make it unit matrix. */
  116.      Mat[0][0] = CosTeta;
  117.      Mat[0][1] = SinTeta;
  118.      Mat[1][0] = -SinTeta;
  119.      Mat[1][1] = CosTeta;
  120. }
  121.  
  122. /*****************************************************************************
  123. * Routine to multiply 2 4by4 matrices:                                       *
  124. * Note MatRes might be one of Mat1 or Mat2 - it is only updated in the end!  *
  125. *****************************************************************************/
  126. void MultTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2)
  127. {
  128.     int i, j, k;
  129.     MatrixType MatResTemp;
  130.  
  131.     for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) {
  132.         MatResTemp[i][j] = 0;
  133.         for (k = 0; k < 4; k++) MatResTemp[i][j] += Mat1[i][k] * Mat2[k][j];
  134.     }
  135.     for (i = 0; i < 4; i++) for (j = 0; j < 4; j++)
  136.     MatRes[i][j] = MatResTemp[i][j];
  137. }
  138.  
  139. /*****************************************************************************
  140. * Routine to multiply Vector by 4by4 matrix:                                 *
  141. * The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1    *
  142. * Note VRes might be Vec as it is only updated in the end.                   *
  143. *****************************************************************************/
  144. void MultVecby4by4(RealType VRes[3], RealType Vec[3], MatrixType Mat)
  145. {
  146.     int i, j;
  147.     RealType CalcW = Mat[3][3];
  148.     RealType VTemp[3];
  149.  
  150.     for (i = 0; i < 3; i++) {
  151.         VTemp[i] = Mat[3][i];         /* Initiate it with the weight factor. */
  152.         for (j = 0; j < 3; j++) VTemp[i] += Vec[j] * Mat[j][i];
  153.     }
  154.  
  155.     for (i = 0; i < 3; i++) CalcW += Vec[i] * Mat[i][3];
  156.     if (CalcW == 0) CalcW = 1 / INFINITY;
  157.  
  158.     for (i = 0; i < 3; i++) VRes[i] = VTemp[i]/CalcW;
  159. }
  160.